perm filename OPERAT.SAI[PNT,HE]1 blob
sn#326345 filedate 1978-01-04 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00010 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00003 00002 ENTRY
C00005 00003 ! computes C←A*B, where A=array, B,C = vectors[1:3]
C00007 00004 ! angles: setrot,xyzrot,axisrt,eulero,decode
C00013 00005 ! arithmetic: operations on matrices,assignment/extraction of values
C00016 00006 INTERNAL RPTR(VECTOR) PROCEDURE VMAKE(RPTR(SCALAR) R1,R2,R3)
C00024 00007 ! frame tree: unlnk_node, is_ancestor, LINKFR
C00026 00008 ! arithmetic: absxf, setabs, absset, relset, absloc
C00030 00009 ! this is for scalar assignment instructions:
C00049 00010 ! procedures returning a record pointer
C00059 ENDMK
C⊗;
ENTRY;
BEGIN "OPERAT"
REQUIRE"MACROS.SAI[PNT,HE]"SOURCE_FILE;
REQUIRE "RECORD.DEF[PNT,HE]" SOURCE_FILE;
! in MAINPR.SAI[PNT,HE];
EXTERNAL REAL $EPS;
EXTERNAL PROCEDURE ABORT1(STRING NAME,ERROR);
EXTERNAL RPTR (SCALAR,VECTOR,ROT,FRAME,TRANS) PROCEDURE MK_REC(INTEGER TYPE);
! in KILLER.SAI or in POINTY.SAI if KILL not defined;
EXTERNAL PROCEDURE SAVFR(RPTR(FRAME)N);
! computes C←A*B, where A=array, B,C = vectors[1:3];
INTERNAL SIMPLE PROCEDURE XFVT(REAL ARRAY A,B,C);
BEGIN
INTEGER I,K;OWN REAL ARRAY TEMP[1:4];
FOR I←1 STEP 1 UNTIL 3 DO TEMP[I]←B[I];
TEMP[4]←1;
ARRCLR(C);
FOR I←1 STEP 1 UNTIL 3 DO
FOR K←1 STEP 1 UNTIL 4 DO C[I]←C[I]+A[I,K]*TEMP[K];
END;
! computes C ← A*B;
INTERNAL SIMPLE PROCEDURE XFXF(REAL ARRAY A,B,C);
BEGIN
INTEGER I,J,K;
ARRCLR(C);
FOR I←1 STEP 1 UNTIL 3 DO
FOR J←1 STEP 1 UNTIL 4 DO
BEGIN
FOR K←1 STEP 1 UNTIL 4 DO C[I,J]←C[I,J]+A[I,K]*B[K,J];
END;
C[4,4]←1.0;
C[5,4]←0; ! angles are not valid;
END;
! computes B ← inv(A);
INTERNAL SIMPLE PROCEDURE XFINV(REAL ARRAY A,B);
BEGIN
INTEGER I,J;
ARRCLR(B);
FOR I←1 STEP 1 UNTIL 3 DO
FOR J ← 1 STEP 1 UNTIL 3 DO
BEGIN
B[I,J]←A[J,I];
B[I,4]←B[I,4]-B[I,J]*A[J,4];
END;
B[4,4]←1.0;
B[5,4]←0;
END;
! computes C ← inv(A)*B;
INTERNAL SIMPLE PROCEDURE INVXFX(REAL ARRAY A,B,C);
BEGIN
OWN REAL ARRAY XFTMP[1:5,1:4];
XFINV(A,XFTMP);
XFXF(XFTMP,B,C);
END;
! angles: setrot,xyzrot,axisrt,eulero,decode;
! computes the rotation part of XF to correspond to
ROT(Z,TH)*ROT(Y,PH)*ROT(Z,W), where the values of the angles are
expressed in degrees;
INTERNAL SIMPLE PROCEDURE SETROT(REAL ARRAY XF;REAL W,PH,TH);
BEGIN "SETROT"
REAL SW,CW,SPH,CPH,ST,CT;
SW←SIND(W);CW←COSD(W);
SPH←SIND(PH);CPH←COSD(PH);
ST←SIND(TH);CT←COSD(TH);
XF[1,1]←CW*CPH*CT-SW*ST;XF[1,2]←-CW*ST-SW*CPH*CT;XF[1,3]←SPH*CT;
XF[2,1]←CW*CPH*ST+SW*CT;XF[2,2]←CW*CT-SW*CPH*ST;XF[2,3]←SPH*ST;
XF[3,1]←-CW*SPH;XF[3,2]←SW*SPH;XF[3,3]←CPH;
XF[5,1]←W;XF[5,2]←PH;XF[5,3]←TH;
XF[5,4]←1.0;
END "SETROT";
! computes the rotation part of XF to correspond to the rotation
by angle (in degrees) about axis (only XHAT, YHAT, ZHAT);
INTERNAL SIMPLE PROCEDURE XYZROT(REAL ARRAY XF; REAL ANGLE,CX,CY,CZ);
BEGIN "X"
REAL SVAL,CVAL,W,PH,TH;
SVAL←SIND(ANGLE);
CVAL←COSD(ANGLE);
XF[5,4]←0.;
XF[1,1]←CVAL+(1-CVAL)*CX↑2;
XF[2,2]←CVAL+(1-CVAL)*CY↑2;
XF[3,3]←CVAL+(1-CVAL)*CZ↑2;
XF[1,2]←(1-CVAL)*CX*CY-CZ*SVAL;
XF[2,1]←(1-CVAL)*CX*CY+CZ*SVAL;
XF[1,3]←(1-CVAL)*CX*CZ+CY*SVAL;
XF[3,1]←(1-CVAL)*CX*CZ-CY*SVAL;
XF[2,3]←(1-CVAL)*CY*CZ-CX*SVAL;
XF[3,2]←(1-CVAL)*CY*CZ+CX*SVAL;
END "X";
! returns the values of the angles (in degrees) from the rotation part
of XF. If the angles are valid (XF[5,4]>0) their values are in the fifth
row of XF, otherwise they have to be recomputed;
INTERNAL PROCEDURE EULERO(REAL ARRAY XF;REFERENCE REAL W,PH,TH);
BEGIN "EULERO"
! REAL EPS;! EPS←0.001;
IF XF[5,4]>0 THEN
BEGIN
W←XF[5,1];PH←XF[5,2];TH←XF[5,3];
END
ELSE
BEGIN
REAL SPH,CTH;
! since the function ATAN2 returns the value in radians
conversions to degrees are required;
PH←ATAN2(SQRT(XF[1,3]↑2 + XF[2,3]↑2),XF[3,3]);
PH←PH/#DEG; ! converts to degrees;
SPH←SIND(PH);
IF ABS(SPH)<$EPS THEN
BEGIN
PH←IF XF[3,3]>0 THEN 0 ELSE 180;
TH←0;
W←ATAN2(XF[2,1],XF[2,2]);
W←W/#DEG; ! converts to degrees;
SETROT(XF,W,PH,TH);
END
ELSE
BEGIN
W←ATAN2(XF[3,2],-XF[3,1]);
TH←ATAN2(XF[2,3],XF[1,3]);
W←W/#DEG; TH←TH/#DEG;
CTH←COSD(TH);
PH← IF $EPS<abs(CTH)
THEN ATAN2(XF[1,3]/CTH,XF[3,3])
ELSE ATAN2(XF[2,3]/SIND(TH),XF[3,3]);
PH←PH/#DEG; ! converts to degrees;
XF[5,1]←W;XF[5,2]←PH;XF[5,3]←TH;
XF[5,4]←1.0;
END;
END ;
END "EULERO";
! decodes the rotation matrix as a product of rotations about
the three main axes (used by ↑);
INTERNAL PROCEDURE DECODE (REAL ARRAY XF; REFERENCE REAL A,B,C);
BEGIN "DECODE"
REAL SA,CB;! real EPS ! EPS←0.001;
IF ABS(XF[3,1])≤1
THEN B←ASIN(-XF[3,1])/#DEG
ELSE B←ATAN2(-XF[3,1],SQRT(XF[3,2]↑2 + XF[3,3]↑2))/#DEG;
CB←COSD(B);
IF ABS(CB)<$EPS THEN
BEGIN
A←0;
C←ATAN2(XF[2,3],XF[1,3])/#DEG;
IF XF[3,1]<0 THEN B←90 ELSE B←-90;
END
ELSE BEGIN
C←ATAN2(XF[2,1],XF[1,1])/#DEG;
A←ATAN2(XF[3,2],XF[3,3])/#DEG;
SA←SIND(A);
IF B≠0 THEN
IF ABS(SA)≥$EPS
THEN B←ATAN2(-XF[3,1],XF[3,2]/SA)/#DEG
ELSE B←ATAN2(-XF[3,1],XF[3,3]/COSD(A))/#DEG;
END;
END "DECODE";
! arithmetic: operations on matrices,assignment/extraction of values;
! computes the norm of comp and returns results in comp;
INTERNAL RPTR(VECTOR) PROCEDURE NORMVT(RPTR(VECTOR)EL);
BEGIN
REAL M;RPTR(VECTOR)TEMP;
M←SQRT(VECTOR:XC[EL]↑2+VECTOR:YC[EL]↑2+VECTOR:ZC[EL]↑2);
IF M≤$EPS THEN ABORT1("NORM NOT WELL DEFINED"," ");
TEMP←MK_REC(#VT);
VECTOR:XC[TEMP]←VECTOR:XC[EL]/M;
VECTOR:YC[TEMP]←VECTOR:YC[EL]/M;
VECTOR:ZC[TEMP]←VECTOR:ZC[EL]/M;
RETURN(TEMP);
END;
! computes the norm of FIRST minus SECOND, and returns values in RESULT;
INTERNAL RPTR(VECTOR) PROCEDURE NMSUB(RPTR(VECTOR) V1,V2);
BEGIN
RPTR(VECTOR)TEMP;
TEMP←MK_REC(#VT);
VECTOR:XC[TEMP]←VECTOR:XC[V1]-VECTOR:XC[V2];
VECTOR:YC[TEMP]←VECTOR:YC[V1]-VECTOR:YC[V2];
VECTOR:ZC[TEMP]←VECTOR:ZC[V1]-VECTOR:ZC[V2];
TEMP←NORMVT(TEMP);
RETURN(TEMP);
END;
! computπs the norm of the cross product of FIRST and SECOND, and returns
values in RESULT;
INTERNAL RPTR(VECTOR) PROCEDURE NMCROS(RPTR(VECTOR)V1,V2);
BEGIN
RPTR(VECTOR)TEMP;
TEMP←MK_REC(#VT);
VECTOR:XC[TEMP]←VECTOR:YC[V1]*VECTOR:ZC[V2]
-VECTOR:ZC[V1]*VECTOR:YC[V2];
VECTOR:YC[TEMP]←VECTOR:ZC[V1]*VECTOR:XC[V2]
-VECTOR:XC[V1]*VECTOR:ZC[V2];
VECTOR:ZC[TEMP]←VECTOR:XC[V1]*VECTOR:YC[V2]
-VECTOR:YC[V1]*VECTOR:XC[V2];
TEMP←NORMVT(TEMP);
RETURN(TEMP);
END;
! puts in the appropriate fields of the vector pointed by el the
values contained in the array comp;
INTERNAL SIMPLE PROCEDURE PUTVTV (RPTR(VECTOR) EL; REAL ARRAY COMP);
BEGIN
VECTOR:XC[EL]←COMP[1];
VECTOR:YC[EL]←COMP[2];
VECTOR:ZC[EL]←COMP[3];
END;
! returns in the array comp the components of the vector pointed by el;
INTERNAL SIMPLE PROCEDURE GETVTV(RPTR(VECTOR) EL; REAL ARRAY COMP);
BEGIN
COMP[1]←VECTOR:XC[EL];
COMP[2]←VECTOR:YC[EL];
COMP[3]←VECTOR:ZC[EL];
END;
INTERNAL RPTR(VECTOR) PROCEDURE VMAKE(RPTR(SCALAR) R1,R2,R3);
BEGIN
RPTR(VECTOR)V1; V1←MK_REC(#VT);
VECTOR:XC[V1]←SCALAR:VALUE[R1];
VECTOR:YC[V1]←SCALAR:VALUE[R2];
VECTOR:ZC[V1]←SCALAR:VALUE[R3];
RETURN(V1);
END;
INTERNAL RPTR(ROT)PROCEDURE RMAKE(RPTR(VECTOR)AX;RPTR(SCALAR)ANGLE);
BEGIN
RPTR(ROT)RTEMP;RPTR(VECTOR)VTEMP;REAL X,Y,Z;
RTEMP←MK_REC(#RT);
VTEMP←NORMVT(AX);
XYZROT(ROT:XF[RTEMP],SCALAR:VALUE[ANGLE],
VECTOR:XC[VTEMP],VECTOR:YC[VTEMP],VECTOR:ZC[VTEMP]);
RETURN(RTEMP);
END;
INTERNAL RPTR(TRANS) PROCEDURE TMAKE(RPTR(ROT)TMPRT;RPTR(VECTOR)TMPVT);
BEGIN "B"
RPTR(TRANS) XFE;
XFE←MK_REC(#TR);
ARRTRAN(TRANS:XF[XFE],ROT:XF[TMPRT]);
TRANS:XF[XFE][1,4]←VECTOR:XC[TMPVT];
TRANS:XF[XFE][2,4]←VECTOR:YC[TMPVT];
TRANS:XF[XFE][3,4]←VECTOR:ZC[TMPVT];
RETURN(XFE);
END "B";
INTERNAL RPTR(FRAME)PROCEDURE FMAKE(RPTR(ROT)TMPRT;RPTR(VECTOR)TMPVT);
BEGIN "B"
RPTR(FRAME) XFE;
XFE←MK_REC(#FR);
ARRTRAN(FRAME:XF[XFE],ROT:XF[TMPRT]);
FRAME:XF[XFE][1,4]←VECTOR:XC[TMPVT];
FRAME:XF[XFE][2,4]←VECTOR:YC[TMPVT];
FRAME:XF[XFE][3,4]←VECTOR:ZC[TMPVT];
RETURN(XFE);
END "B";
! frame tree: unlnk_node, is_ancestor, LINKFR;
! breaks links in frame tree for the frame N;
INTERNAL PROCEDURE UNLINK(RPTR(FRAME) N);
BEGIN
RPTR(FRAME) Y,E;
E←FRAME:EBRO[N];
IF (Y←FRAME:YBRO[N])=NULL_RECORD
THEN BEGIN
IF FRAME:DAD[N]≠NULL_RECORD THEN
FRAME:SON[FRAME:DAD[N]]←E;
END
ELSE FRAME:EBRO[Y]←E;
IF E≠NULL_RECORD THEN
FRAME:YBRO[E]←Y;
FRAME:EBRO[N]←NULL_RECORD;
FRAME:YBRO[N]←NULL_RECORD;
FRAME:DAD[N]←NULL_RECORD;
END;
! returns true if D is an ancestor of N;
BOOLEAN PROCEDURE IS_ANCESTOR(RPTR(FRAME) N,D);
BEGIN
WHILE N≠NULL_RECORD DO
IF N=D
THEN RETURN(TRUE)
ELSE N←FRAME:DAD[N];
RETURN(FALSE);
END;
! sets #UP pointer structure in frame tree for N to be a child of D;
INTERNAL PROCEDURE LINKFR(RPTR(FRAME) N,D);
BEGIN
IF NOT(D=F_WRLD AND FRAME:HOWLINKED[N]=#INDLK)
THEN IF IS_ANCESTOR(D,N)
THEN ABORT1(" backwards affixment to",frame:pname[D]);
IF FRAME:DAD[N]≠NULL_RECORD
THEN UNLINK(N);
IF (FRAME:EBRO[N]←FRAME:SON[D])≠NULL_RECORD THEN
FRAME:YBRO[FRAME:EBRO[N]]←N;
FRAME:YBRO[N]←NULL_RECORD;
FRAME:DAD[N]←D;
FRAME:SON[D]←N;
END;
! arithmetic: absxf, setabs, absset, relset, absloc;
! sets up xf to be the location of N in the WORLD;
INTERNAL PROCEDURE ABSXF(RPTR(FRAME) N;REAL ARRAY XF);
BEGIN
ARRTRAN(XF,FRAME:XF[N]); ! xf ← frame:xf[N];
WHILE FRAME:HOWLINKED[N]≠#INDLK DO
BEGIN
OWN REAL ARRAY XFTMP[1:5,1:4];
N←FRAME:DAD[N];
IF N=NULL_RECORD
THEN ABORT1(" "," incorrect tree structure");
XFXF(FRAME:XF[N],XF,XFTMP); ! xftmp ← xf[n]*xf;
ARRTRAN(XF,XFTMP); ! xf ← xftmp;
END;
END;
! sets up link transforms so that ABSXF(N)=XF.
(If rigid affixments, will move parents);
INTERNAL PROCEDURE SETABS(RPTR(FRAME) N;REAL ARRAY XF);
BEGIN
OWN REAL ARRAY XFTMP,XFTMP2,XFTMP3[1:5,1:4];
RPTR(SYMBOL)EL;RPTR(FRAME) TEMP;
TEMP←N;
ARRTRAN(XFTMP,XF); ! xftmp←xf;
WHILE FRAME:HOWLINKED[N]=#RGDLK DO
BEGIN
XFINV(frame:XF[N],XFTMP3);
XFXF(XFTMP,XFTMP3,XFTMP2);
ARRTRAN(XFTMP,XFTMP2); ! xftmp←xftmp*inv(xf[n]);
N←FRAME:DAD[N];
END;
IF TEMP≠N THEN SAVFR(N);
IF FRAME:HOWLINKED[N]=#INDLK
THEN ARRTRAN(FRAME:XF[N],XFTMP)
ELSE BEGIN
! xftmp2 gets the absolute value of dad of N;
ABSXF(FRAME:DAD[N],XFTMP2);
! frame:xf[n]←inv(xftmp2)*xftmp;
INVXFX(XFTMP2,XFTMP,FRAME:XF[N]);
END;
END;
! sets the relative value of the frame to the value of TRANS;
INTERNAL PROCEDURE RELSET(RPTR(FRAME)FRA;RPTR(TRANS)XFE);
BEGIN
ARRTRAN(FRAME:XF[FRA],TRANS:XF[XFE]);
END;
! sets the absolute value to the frame to be the value of TRANS;
INTERNAL PROCEDURE ABSSET(RPTR(FRAME) FRA;RPTR(TRANS)XFE);
BEGIN
SETABS(FRA,TRANS:XF[XFE]);
END;
! returns a TRANS with the absolute value of the frame;
INTERNAL RPTR(TRANS) PROCEDURE ABSLOC(RPTR(FRAME) ND);
BEGIN
RPTR(TRANS) XFE;
XFE←MK_REC(4); ! SHOULD BE #TR;
ABSXF(ND,TRANS:XF[XFE]);
RETURN (XFE);
END;
! this is for scalar assignment instructions:
el ← num1 op num2
where op is the operator, num1,num2 are real numbers, el is a scalar;
INTERNAL RPTR(SCALAR)PROCEDURE OPSCAL(RPTR(SCALAR)VAL1,VAL2;STRING OP);
BEGIN
RPTR(SCALAR)VALF;
VALF←MK_REC(#SC);
IF OP="+"
THEN SCALAR:VALUE[VALF]← SCALAR:VALUE[VAL1]+SCALAR:VALUE[VAL2]
ELSE IF OP="-"
THEN SCALAR:VALUE[VALF]← SCALAR:VALUE[VAL1]-SCALAR:VALUE[VAL2]
ELSE IF OP="*"
THEN SCALAR:VALUE[VALF]← SCALAR:VALUE[VAL1]*SCALAR:VALUE[VAL2]
ELSE SCALAR:VALUE[VALF]← SCALAR:VALUE[VAL1]/SCALAR:VALUE[VAL2];
RETURN(VALF);
END;
! this is for vector assignment instruction:
valf ← val op num,
where op is the operator, num is a scalar, val,valf are vectors;
INTERNAL RPTR(VECTOR)PROCEDURE OPSCVT(RPTR(SCALAR) NUM;RPTR(VECTOR)VAL;STRING OP);
BEGIN
RPTR(VECTOR)VALF;
VALF←MK_REC(#VT);
IF OP="*"
THEN BEGIN
VECTOR:XC[VALF]←VECTOR:XC[VAL]*SCALAR:VALUE[NUM];
VECTOR:YC[VALF]←VECTOR:YC[VAL]*SCALAR:VALUE[NUM];
VECTOR:ZC[VALF]←VECTOR:ZC[VAL]*SCALAR:VALUE[NUM];
END
ELSE BEGIN
VECTOR:XC[VALF]←VECTOR:XC[VAL]/SCALAR:VALUE[NUM];
VECTOR:YC[VALF]←VECTOR:YC[VAL]/SCALAR:VALUE[NUM];
VECTOR:ZC[VALF]←VECTOR:ZC[VAL]/SCALAR:VALUE[NUM];
END;
RETURN(VALF);
END;
! this is for the dot product operation:
valf←val1.val2
where valf is the scalar, val1,val2 are vectors;
INTERNAL RPTR(SCALAR)PROCEDURE OPDOT(RPTR(VECTOR)VAL1,VAL2);
BEGIN
RPTR(SCALAR)VALF;
VALF←MK_REC(#SC);
SCALAR:VALUE[VALF]←VECTOR:XC[VAL1]*VECTOR:XC[VAL2]+
VECTOR:YC[VAL1]*VECTOR:YC[VAL2]+
VECTOR:ZC[VAL1]*VECTOR:ZC[VAL2];
RETURN(VALF);
END;
! this is for vector assignment operation:
valf ← val1 op val2
where val1,val2,valf are vectors and op is the operator;
INTERNAL RPTR(VECTOR)PROCEDURE OPVET(RPTR(VECTOR) VAL1,VAL2;STRING OP);
BEGIN
RPTR(VECTOR)VALF;
VALF←MK_REC(#VT);
IF OP="+"
THEN BEGIN
VECTOR:XC[VALF]←VECTOR:XC[VAL1]+VECTOR:XC[VAL2];
VECTOR:YC[VALF]←VECTOR:YC[VAL1]+VECTOR:YC[VAL2];
VECTOR:ZC[VALF]←VECTOR:ZC[VAL1]+VECTOR:ZC[VAL2];
END
ELSE BEGIN
VECTOR:XC[VALF]←VECTOR:XC[VAL1]-VECTOR:XC[VAL2];
VECTOR:YC[VALF]←VECTOR:YC[VAL1]-VECTOR:YC[VAL2];
VECTOR:ZC[VALF]←VECTOR:ZC[VAL1]-VECTOR:ZC[VAL2];
END;
RETURN(VALF);
END;
! this is for rotation assignment operation:
valf ← val1 * val2
where val1,val2,valf are rotations;
INTERNAL RPTR(ROT)PROCEDURE OPRTRT(RPTR(ROT)VAL1,VAL2);
BEGIN
RPTR(ROT)VALF;
VALF←MK_REC(#RT);
XFXF(ROT:XF[VAL1],ROT:XF[VAL2],ROT:XF[VALF]);
RETURN(VALF);
END;
! this is for product of a vector and a rotation:
valf ← val1 * val2
where val2,valf are vectors and val1 is a rotation;
INTERNAL RPTR(VECTOR)PROCEDURE OPRTVT(RPTR(ROT)VAL1;RPTR(VECTOR)VAL2);
BEGIN
RPTR(VECTOR)VALF;
REAL ARRAY COMPF,COMP2[1:3];
VALF←MK_REC(#VT);
GETVTV(VAL2,COMP2);
XFVT(ROT:XF[VAL1],COMP2,COMPF);
PUTVTV(VALF,COMPF);
RETURN(VALF);
END;
! this is for frame translations:
valf ← val1 op val2 (commutative)
where valf,val2 are frames, val1 is a vector and op is the operator;
INTERNAL RPTR(FRAME)PROCEDURE OPFRVT(RPTR(VECTOR) VAL1;RPTR(FRAME)VAL2;STRING OP);
BEGIN
RPTR(FRAME)VALF;
REAL ARRAY FXF[1:5,1:4];
VALF←MK_REC(#FR);
ABSXF(VAL2,FXF);
SETABS(VALF,FXF);
IF OP="+"
THEN BEGIN
FRAME:XF[VALF][1,4]←FRAME:XF[VALF][1,4]+VECTOR:XC[VAL1];
FRAME:XF[VALF][2,4]←FRAME:XF[VALF][2,4]+VECTOR:YC[VAL1];
FRAME:XF[VALF][3,4]←FRAME:XF[VALF][3,4]+VECTOR:ZC[VAL1];
END
ELSE BEGIN
FRAME:XF[VALF][1,4]←FRAME:XF[VALF][1,4]-VECTOR:XC[VAL1];
FRAME:XF[VALF][2,4]←FRAME:XF[VALF][2,4]-VECTOR:YC[VAL1];
FRAME:XF[VALF][3,4]←FRAME:XF[VALF][3,4]-VECTOR:ZC[VAL1];
END;
RETURN(VALF);
END;
! this is to compute a vector when its origin is translated into
the frame (equivalent to valf← val2 REL val1)
valf←val1*val2
where val1 is a frame, val2 and valf are vectors;
INTERNAL RPTR(VECTOR) PROCEDURE OPVTFR(RPTR(FRAME) VAL1;RPTR(VECTOR)VAL2);
BEGIN
REAL ARRAY COMP2,COMPF[1:3];REAL ARRAY FXF[1:5,1:4];RPTR(VECTOR)VALF;
VALF←MK_REC(#VT);
ABSXF(VAL1,FXF);
GETVTV(VAL2,COMP2);
XFVT(FXF,COMP2,COMPF);
PUTVTV(VALF,COMPF);
RETURN(VALF);
END;
! computes <vector>←<trans>*<vector>;
INTERNAL RPTR(VECTOR)PROCEDURE OPTRVT(RPTR(TRANS)VAL1;RPTR(VECTOR)VAL2);
BEGIN
REAL ARRAY COMP2,COMPF[1:3];REAL ARRAY FXF[1:5,1:4];RPTR(VECTOR)VALF;
VALF←MK_REC(#VT);
GETVTV(VAL2,COMP2);
ARRTRAN(FXF,TRANS:XF[VAL1]);
XFVT(FXF,COMP2,COMPF);
PUTVTV(VALF,COMPF);
END;
! computes <trans>←<frame>→<frame>;
INTERNAL RPTR(TRANS)PROCEDURE OPFRFR(RPTR(FRAME)VAL1,VAL2);
BEGIN
RPTR(TRANS) TEMP1,TEMP2,VALF;
TEMP1←ABSLOC(VAL1);
TEMP2←ABSLOC(VAL2);
VALF←MK_REC(#TR);
INVXFX(TRANS:XF[TEMP1],TRANS:XF[TEMP2],TRANS:XF[VALF]);
RETURN(VALF);
END;
! computes <trans>←<trans>*<trans>;
INTERNAL RPTR(TRANS)PROCEDURE OPTRTR(RPTR(TRANS)VAL1,VAL2);
BEGIN
RPTR(TRANS)VALF;
VALF←MK_REC(#TR);
XFXF(TRANS:XF[VAL1],TRANS:XF[VAL2],TRANS:XF[VALF]);
RETURN(VALF);
END;
! computes <frame>← <trans>*<frame>;
INTERNAL RPTR(FRAME)PROCEDURE OPTRFR(RPTR(TRANS)VAL1;RPTR(FRAME)VAL2);
BEGIN
OWN REAL ARRAY FTEMP,FTEMP2[1:5,1:4];RPTR(FRAME)VALF;
ABSXF(VAL2,FTEMP); ! computes absolute value of val2;
XFXF(TRANS:XF[VAL1],FTEMP,FTEMP2); ! ftemp2 abs.pos. of valf;
VALF←MK_REC(#FR);
SETABS(VALF,FTEMP2); ! sets abs.pos of valf to ftemp2;
RETURN(VALF);
END;
! computes <frame>←<frame>*<frame>;
INTERNAL RPTR(FRAME) PROCEDURE OPFR(RPTR(FRAME)VAL1,VAL2);
BEGIN
RPTR(TRANS)TEMP;RPTR(FRAME)VALF;
TEMP←ABSLOC(VAL1);
VALF←OPTRFR(TEMP,VAL2);
RETURN(VALF);
END;
! procedures returning a record pointer;
! returns in the record rot the rotation part of the frame sec;
INTERNAL RPTR(ROT)PROCEDURE FORIEN(RPTR(FRAME)SEC);
BEGIN
RPTR(TRANS) XFE;RPTR(ROT)FIRST;
FIRST←MK_REC(#RT);
XFE←ABSLOC(SEC);
ARRTRAN(ROT:XF[FIRST],TRANS:XF[XFE]);
ROT:XF[FIRST][1,4]←ROT:XF[FIRST][2,4]←ROT:XF[FIRST][3,4]←0.;
RETURN(FIRST);
END;
! returns in the record vector the location part of the frame sec;
INTERNAL RPTR(VECTOR) PROCEDURE FPOS(RPTR(FRAME)SEC);
BEGIN
RPTR(TRANS) XFE;RPTR(VECTOR)FIRST;
XFE←ABSLOC(SEC); ! absolute value of the frame;
FIRST←MK_REC(#VT);
VECTOR:XC[FIRST]←TRANS:XF[XFE][1,4];
VECTOR:YC[FIRST]←TRANS:XF[XFE][2,4];
VECTOR:ZC[FIRST]←TRANS:XF[XFE][3,4];
RETURN(FIRST);
END;
INTERNAL RPTR(VECTOR) PROCEDURE TPOS(RPTR(TRANS)XFE);
BEGIN
RPTR(VECTOR) FIRST;
FIRST←MK_REC(#VT);
VECTOR:XC[FIRST]←TRANS:XF[XFE][1,4];
VECTOR:YC[FIRST]←TRANS:XF[XFE][2,4];
VECTOR:ZC[FIRST]←TRANS:XF[XFE][3,4];
RETURN(FIRST);
END;
INTERNAL RPTR(SCALAR)PROCEDURE SMOD(RPTR(SCALAR)RIGHT);
BEGIN
RPTR(SCALAR)TEMP;
TEMP←MK_REC(#SC);
SCALAR:VALUE[TEMP]←ABS(SCALAR:VALUE[RIGHT]);
RETURN(TEMP);
END;
INTERNAL RPTR(SCALAR)PROCEDURE VMOD(RPTR(VECTOR)RIGHT);
BEGIN
REAL M;RPTR(SCALAR)TEMP;
M←SQRT(VECTOR:XC[RIGHT]↑2+VECTOR:YC[RIGHT]↑2+VECTOR:ZC[RIGHT]↑2);
IF M≤$EPS THEN ABORT1("NORM NOT WELL DEFINED"," ");
TEMP←MK_REC(#SC);
SCALAR:VALUE[TEMP]←M;
RETURN(TEMP);
END;
INTERNAL RPTR(VECTOR)PROCEDURE WRTVT(RPTR(VECTOR) VET;RPTR(FRAME) RELFR);
BEGIN
RPTR(VECTOR)TEMP;
TEMP←OPVTFR(RELFR,VET);
TEMP←OPVET(TEMP,FPOS(RELFR),"-");
RETURN(TEMP);
END;
INTERNAL RPTR(VECTOR)PROCEDURE RELVT(RPTR(VECTOR) VET;RPTR(FRAME) RELFR);
BEGIN
RPTR(VECTOR)TEMP;
TEMP←OPVTFR(RELFR,VET);
RETURN(TEMP);
END;
! returns <frame> ← <frame>*<trans>;
INTERNAL RPTR(FRAME) PROCEDURE RELFR(RPTR(FRAME)DAD;RPTR(TRANS)RELPOS);
BEGIN
OWN REAL ARRAY FTEMP,FTEMP2[1:5,1:4];RPTR(FRAME) TEMP;
TEMP←MK_REC(#FR);
ABSXF(DAD,FTEMP); ! computes absolute value of relf;
XFXF(FTEMP,TRANS:XF[RELPOS],FTEMP2); ! ftemp2 abs.pos. of frn;
SETABS(TEMP,FTEMP2);
RETURN(TEMP);
END;
BOOLEAN PROCEDURE PERM(INTEGER I,J);
BEGIN "a"
INTEGER K;
K←(I+1) MOD 3;
IF K=J THEN RETURN(TRUE) ELSE RETURN(FALSE);
END "a";
! constructs a new record TRANS using the three vectors compa,compb,compc:
the first represents the origin of the trans,
the second is on f_axis,
the third is on f_axis - s_axis plane.
The axes are indicated by the numbers xhat=0,yhat=1,zhat=2;
INTERNAL RPTR (TRANS)PROCEDURE VVVTR (RPTR(VECTOR)V1,V2,V3;
INTEGER F_AXIS(2),S_AXIS(0));
BEGIN "A"
RPTR(TRANS)XFE;
INTEGER K; RPTR(VECTOR) VI,VK,VJ,VTT;
! copies the values of array temp in the j column of array TRANS:xf;
PROCEDURE VTCOPY (RPTR(VECTOR)VVV;INTEGER J);
BEGIN
TRANS:XF[XFE][1,J]←VECTOR:XC[VVV];
TRANS:XF[XFE][2,J]←VECTOR:YC[VVV];
TRANS:XF[XFE][3,J]←VECTOR:ZC[VVV];
END;
XFE←MK_REC(#TR);
VTCOPY(V1,4); ! translation part;
VI←NMSUB(V2,V1);
VTT←NMSUB(V3,V1);
IF PERM(F_AXIS,S_AXIS)
THEN BEGIN
K←(S_AXIS+1) MOD 3; ! third axis;
VK←NMCROS(VI,VTT);
VJ←NMCROS(VK,VI);
END
ELSE BEGIN
K←(F_AXIS+1) MOD 3; ! third axis;
VK←NMCROS(VTT,VI);
VJ←NMCROS(VI,VK);
END;
VTCOPY(VI,F_AXIS+1);
VTCOPY(VK,K+1);
VTCOPY(VJ,S_AXIS+1);
TRANS:XF[XFE][5,4]←0; ! angles not valid;
RETURN(XFE);
END "A";
INTERNAL RPTR (FRAME)PROCEDURE CONSV(RPTR(VECTOR)V1,V2,V3);
BEGIN
RPTR(TRANS)TEMP;RPTR(FRAME)FRA;
TEMP←VVVTR(V1,V2,V3); ! constructs a new trans;
FRA←MK_REC(#FR);
ARRTRAN(FRAME:XF[FRA],TRANS:XF[TEMP]);
RETURN(FRA);
END;
INTERNAL RPTR (FRAME)PROCEDURE CONSF(RPTR(FRAME)F1,F2,F3);
BEGIN
RPTR(VECTOR)V1,V2,V3;RPTR(TRANS)TEMP;RPTR(FRAME)FRA;
V1←FPOS(F1);
V2←FPOS(F2);
V3←FPOS(F3);
TEMP←VVVTR(V1,V2,V3); ! constructs a new trans;
FRA←MK_REC(#FR);
ARRTRAN(FRAME:XF[FRA],TRANS:XF[TEMP]);
RETURN(FRA);
END;
END "OPERAT";